from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
from geometry_msgs.msg import Twist,PoseStamped
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
class SimpleMove(Node):
    def __init__(self):
        super().__init__('simple_move')
        self.subscription = self.create_subscription(String,'goal_pose_bt',self.callback,10)
        self.subscription  # 防止被垃圾回收

    def callback(self, msg):
        x_y = msg.data.split(";")
        goal_pose = PoseStamped()
        # 设置导航目标
        goal_pose.header.frame_id = "map"
        goal_pose.pose.position.x = float(x_y[0])
        goal_pose.pose.position.y = float(x_y[1])
        goal_pose.pose.position.z = 0.0

        goal_pose.pose.orientation.x = 0.0
        goal_pose.pose.orientation.y = 0.0
        goal_pose.pose.orientation.z = 0.0
        goal_pose.pose.orientation.w = 1.0
        result = BasicNavigator().getResult()
        if result == TaskResult.SUCCEEDED:
            # 写判断
            print('1111111')
        BasicNavigator().goToPose(goal_pose)


def main(args=None):
    rclpy.init(args=args)
    Initial_pose = PoseStamped()
    Initial_pose.header.frame_id = "map"
    Initial_pose.pose.position.x = 0.0
    Initial_pose.pose.position.y = 0.0
    Initial_pose.pose.position.z = 0.0
    Initial_pose.pose.orientation.x = 0.0
    Initial_pose.pose.orientation.y = 0.0
    Initial_pose.pose.orientation.z = 0.0
    Initial_pose.pose.orientation.w = 1.0
    BasicNavigator().setInitialPose(Initial_pose)
    Simple_Move = SimpleMove()
    rclpy.spin(Simple_Move)
    Simple_Move.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
